
// Copyright (c) 2012, 2013 Lionel MOISAN.
// Copyright (c) 2012, 2013 Pascal MONASSE.
// Copyright (c) 2012, 2013 Pierre MOULON.

// This Source Code Form is subject to the terms of the Mozilla Public
// License, v. 2.0. If a copy of the MPL was not distributed with this
// file, You can obtain one at http://mozilla.org/MPL/2.0/.

#ifndef OPENMVG_ROBUST_ESTIMATOR_ACRANSAC_H_
#define OPENMVG_ROBUST_ESTIMATOR_ACRANSAC_H_

//-------------------
// Generic implementation of ACRANSAC
//-------------------
// The A contrario parametrization have been first explained in [1] and
//  later extended to generic model estimation in [2] (with a demonstration for
//  the homography) and extended and use at large scale for Structure from
//  Motion in [3].
//
//--
//  [1] Lionel Moisan, Berenger Stival,
//  A probalistic criterion to detect rigid point matches between
//  two images and estimate the fundamental matrix.
//  IJCV 04.
//--
//  [2] Lionel Moisan, Pierre Moulon, Pascal Monasse.
//  Automatic Homographic Registration of a Pair of Images,
//    with A Contrario Elimination of Outliers
//  Image Processing On Line (IPOL), 2012.
//  http://dx.doi.org/10.5201/ipol.2012.mmm-oh
//--
//  [3] Pierre Moulon, Pascal Monasse and Renaud Marlet.
//  Adaptive Structure from Motion with a contrario mode estimation.
//  In 11th Asian Conference on Computer Vision (ACCV 2012)
//--

#include <algorithm>
#include <cmath>
#include <iostream>
#include <iterator>
#include <limits>
#include <numeric>
#include <vector>

#include "rand_sampling.hpp"

namespace openMVG {
namespace robust {

/// logarithm (base 10) of binomial coefficient
static double logcombi(size_t k, size_t n) {
    if (k >= n || k <= 0)
        return (0.0);
    if (n - k < k)
        k = n - k;
    double r = 0.0;
    for (size_t i = 1; i <= k; i++)
        r += log10((double)(n - i + 1)) - log10((double)i);

    return r;
}

/// tabulate logcombi(.,n)
template <typename Type> static void makelogcombi_n(size_t n, std::vector<Type> &l) {
    l.resize(n + 1);
    for (size_t k = 0; k <= n; k++)
        l[k] = static_cast<Type>(logcombi(k, n));
}

/// tabulate logcombi(k,.)
template <typename Type> static void makelogcombi_k(size_t k, size_t nmax, std::vector<Type> &l) {
    l.resize(nmax + 1);
    for (size_t n = 0; n <= nmax; n++)
        l[n] = static_cast<Type>(logcombi(k, n));
}

/// Distance and associated index
typedef std::pair<double, size_t> ErrorIndex;

/// Find best NFA and its index wrt square error threshold in e.
static ErrorIndex bestNFA(int startIndex, // number of point required for estimation
                          double logalpha0, const std::vector<ErrorIndex> &e, double loge0, double maxThreshold,
                          const std::vector<float> &logc_n, const std::vector<float> &logc_k, double multError = 1.0) {
    ErrorIndex bestIndex(std::numeric_limits<double>::infinity(), startIndex);
    const size_t n = e.size();
    for (size_t k = startIndex + 1; k <= n && e[k - 1].first <= maxThreshold; ++k) {
        double logalpha = logalpha0 + multError * log10(e[k - 1].first + std::numeric_limits<float>::min());
        ErrorIndex index(loge0 + logalpha * (double)(k - startIndex) + logc_n[k] + logc_k[k], k);

        if (index.first < bestIndex.first)
            bestIndex = index;
    }
    return bestIndex;
}

/// Pick a random sample
/// \param sizeSample The size of the sample.
/// \param vec_index  The possible data indices.
/// \param sample The random sample of sizeSample indices (output).
static void UniformSample(int sizeSample, const std::vector<size_t> &vec_index, std::vector<size_t> *sample) {
    sample->resize(sizeSample);
    random_sample(sizeSample, vec_index.size(), sample);
    for (int i = 0; i < sizeSample; ++i)
        (*sample)[i] = vec_index[(*sample)[i]];
}

/**
 * @brief ACRANSAC routine (ErrorThreshold, NFA)
 *
 * @param[in] kernel model and metric object
 * @param[out] vec_inliers points that fit the estimated model
 * @param[in] nIter maximum number of consecutive iterations
 * @param[out] model returned model if found
 * @param[in] precision upper bound of the precision (squared error)
 * @param[in] bVerbose display console log
 *
 * @return (errorMax, minNFA)
 */
template <typename Kernel>
std::pair<double, double> ACRANSAC(const Kernel &kernel, std::vector<size_t> &vec_inliers, size_t nIter = 1024,
                                   typename Kernel::Model *model = NULL,
                                   double precision = std::numeric_limits<double>::infinity(), bool bVerbose = false) {
    vec_inliers.clear();

    const size_t sizeSample = Kernel::MINIMUM_SAMPLES;
    const size_t nData = kernel.NumSamples();
    if (nData <= (size_t)sizeSample)
        return std::make_pair(0.0, 0.0);

    const double maxThreshold = (precision == std::numeric_limits<double>::infinity())
                                    ? std::numeric_limits<double>::infinity()
                                    : precision * kernel.normalizer2()(0, 0) * kernel.normalizer2()(0, 0);

    std::vector<ErrorIndex> vec_residuals(nData); // [residual,index]
    std::vector<double> vec_residuals_(nData);
    std::vector<size_t> vec_sample(sizeSample); // Sample indices

    // Possible sampling indices [0,..,nData] (will change in the optimization
    // phase)
    std::vector<size_t> vec_index(nData);
    std::iota(vec_index.begin(), vec_index.end(), 0);

    // Precompute log combi
    double loge0 = log10((double)Kernel::MAX_MODELS * (nData - sizeSample));
    std::vector<float> vec_logc_n, vec_logc_k;
    makelogcombi_n(nData, vec_logc_n);
    makelogcombi_k(sizeSample, nData, vec_logc_k);

    // Output parameters
    double minNFA = std::numeric_limits<double>::infinity();
    double errorMax = std::numeric_limits<double>::infinity();

    // Reserve 10% of iterations for focused sampling
    size_t nIterReserve = nIter / 10;
    nIter -= nIterReserve;

    // Main estimation loop.
    for (size_t iter = 0; iter < nIter; ++iter) {
        UniformSample(sizeSample, vec_index, &vec_sample); // Get random sample

        std::vector<typename Kernel::Model> vec_models; // Up to max_models solutions
        kernel.Fit(vec_sample, &vec_models);

        // Evaluate models
        bool better = false;
        for (size_t k = 0; k < vec_models.size(); ++k) {
            // Residuals computation and ordering
            kernel.Errors(vec_models[k], vec_residuals_);
            for (size_t i = 0; i < nData; ++i) {
                const double error = vec_residuals_[i];
                vec_residuals[i] = ErrorIndex(error, i);
            }
            std::sort(vec_residuals.begin(), vec_residuals.end());

            // Most meaningful discrimination inliers/outliers
            const ErrorIndex best = bestNFA(sizeSample, kernel.logalpha0(), vec_residuals, loge0, maxThreshold,
                                            vec_logc_n, vec_logc_k, kernel.multError());

            if (best.first < minNFA /*&& vec_residuals[best.second-1].first < errorMax*/) {
                // A better model was found
                better = true;
                minNFA = best.first;
                vec_inliers.resize(best.second);
                for (size_t i = 0; i < best.second; ++i)
                    vec_inliers[i] = vec_residuals[i].second;
                errorMax = vec_residuals[best.second - 1].first; // Error threshold
                if (model)
                    *model = vec_models[k];

                if (bVerbose) {
                    std::cout << "  nfa=" << minNFA << " inliers=" << best.second << "/" << nData
                              << " precisionNormalized=" << errorMax
                              << " precision=" << kernel.unormalizeError(errorMax) << " (iter=" << iter;
                    std::cout << ",sample=";
                    std::copy(vec_sample.begin(), vec_sample.end(), std::ostream_iterator<size_t>(std::cout, ","));
                    std::cout << ")" << std::endl;
                }
            }
        }

        // ACRANSAC optimization: draw samples among best set of inliers so far
        if ((better && minNFA < 0) || (iter + 1 == nIter && nIterReserve)) {
            if (vec_inliers.empty()) { // No model found at all so far
                nIter++;               // Continue to look for any model, even not meaningful
                nIterReserve--;
            } else {
                // ACRANSAC optimization: draw samples among best set of inliers
                // so far
                vec_index = vec_inliers;
                if (nIterReserve) {
                    nIter = iter + 1 + nIterReserve;
                    nIterReserve = 0;
                }
            }
        }
    }

    if (minNFA >= 0)
        vec_inliers.clear();

    if (!vec_inliers.empty()) {
        if (model)
            kernel.Unnormalize(model);
        errorMax = kernel.unormalizeError(errorMax);
    }

    return std::make_pair(errorMax, minNFA);
}

} // namespace robust
} // namespace openMVG
#endif // OPENMVG_ROBUST_ESTIMATOR_ACRANSAC_H_
